#!/usr/bin/env python3

import rclpy
import cv2
import mediapipe as mp
# import time
from rclpy.node import Node
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
cap = cv2.VideoCapture(0)

bridge = CvBridge()
mpHands = mp.solutions.hands
hands = mpHands.Hands()
mpDraw = mp.solutions.drawing_utils

# pTime = 0
# cTime = 0

class NodeSubscribe(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("大家好，我是%s!" % name)
 
    def callback(self,data):
        cv_img = bridge.imgmsg_to_cv2(data, "bgr8")
        imgRGB = cv2.cvtColor(cv_img, cv2.COLOR_BGR2RGB)  # 2 = to
        results = hands.process(imgRGB)
        # print(results.multi_hand_landmarks)//检查手坐标输出
        if results.multi_hand_landmarks:
            for handLms in results.multi_hand_landmarks:
                for id, lm in enumerate(handLms.landmark):
                    # print(id, lm)
                    h, w, c = cv_img.shape
                    cx, cy = int(lm.x * w), int(lm.y * h)
                    print(id, cx, cy)
                    # if id == 4:
                    cv2.circle(cv_img, (cx, cy), 15, (255, 0, 255), cv2.FILLED)
                mpDraw.draw_landmarks(cv_img, handLms, mpHands.HAND_CONNECTIONS)

        cv2.imshow("hand_pose" , cv_img)
        cv2.waitKey(1)
        
    def callback2(self,data):
        cv_img = bridge.imgmsg_to_cv2(data, "bgr8")
        cv2.imshow("color_depth_frame" , cv_img)
        cv2.waitKey(1)
 

def main(args=None):
    rclpy.init()
    # 建立一个节点(sub_image_node)用来接受以下两个话题中的图像数据
    node = NodeSubscribe("handpose_identify_node")
    # 接收话题image_data中的图像数据，并可视化
    node.create_subscription(Image,'image_data', node.callback, 10)
    # 接收话题image_data2中的图像数据，并可视化
    node.create_subscription(Image,'image_data2', node.callback2, 10)
    
    rclpy.spin(node)
    rclpy.shutdown()

# while True:
#     success, img = cap.read()
#     imgRGB = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)  # 2 = to
#     results = hands.process(imgRGB)
#     # print(results.multi_hand_landmarks)//检查手坐标输出
#     if results.multi_hand_landmarks:
#         for handLms in results.multi_hand_landmarks:
#             for id, lm in enumerate(handLms.landmark):
#                 # print(id, lm)
#                 h, w, c = img.shape
#                 cx, cy = int(lm.x * w), int(lm.y * h)
#                 print(id, cx, cy)
#                 # if id == 4:
#                 cv2.circle(img, (cx, cy), 15, (255, 0, 255), cv2.FILLED)
#             mpDraw.draw_landmarks(img, handLms, mpHands.HAND_CONNECTIONS)

#     cTime = time.time()
#     fps = 1 / (cTime - pTime)
#     pTime = cTime

#     cv2.putText(img, str(int(fps)), (10, 70), cv2.FONT_HERSHEY_PLAIN, 3,
#                 (255, 255, 255), 2)

#     cv2.imshow("Image", img)
#     cv2.waitKey(1)
